#include "testApp.h"


// ----  AR TK+ STUFF - ripped from the single marker demo app
// needs a home in a nice ofxARToolkit addon :)
#include <ARToolKitPlus/TrackerSingleMarker.h>
using ARToolKitPlus::TrackerSingleMarker;
#include <ARToolKitPlus/TrackerMultiMarker.h>

//lmpl no longer exits
//#include "ARToolKitPlus/TrackerSingleMarkerImpl.h"

//#include "ARToolKitPlus/TrackerMultiMarkerImpl.h"




vector<int> markerId ;
//int markerId = 0;
//class MyLogger : public ARToolKitPlus::Logger
//{
//    void artLog(const char* nStr)
//    {
//        printf(nStr);
//    }
//};

static const int       width = 640, height = 480, bpp = 1;
static   size_t        numPixels = width*height*bpp;
static    size_t        numBytesRead;
static   const char    *fName = "data/markerboard_480-499.raw";
static    unsigned char *cameraBuffer = new unsigned char[numPixels];
//static    MyLogger      logger;
static bool useBCH = false;

ARToolKitPlus::TrackerSingleMarker *tracker;

static bool bDraw = false;




GLfloat lightOnePosition[] = {40.0, 40, 100.0, 0.0};
GLfloat lightOneColor[] = {0.99, 0.99, 0.99, 1.0};

GLfloat lightTwoPosition[] = {-40.0, 40, 100.0, 0.0};
GLfloat lightTwoColor[] = {0.99, 0.99, 0.99, 1.0};

float mWidth = width;
float mHeight = height;
//--------------------------------------------------------------
void testApp::setup()
{
    ofSetWindowShape(mWidth,mHeight);


    grabber.initGrabber(640, 480);
    convert.allocate(640, 480);
    gray.allocate(640, 480);



    ofBackground(127,127,127);


    OF.loadFont("frabk.ttf", 48);
    AR.loadFont("frabk.ttf", 48);


    // ----------------------------------  AR TK+ STUFF - ripped from the single marker demo app

    // create a tracker that does:
    //  - 6x6 sized marker images
    //  - samples at a maximum of 6x6
    //  - works with luminance (gray) images
    //  - can load a maximum of 1 pattern
    //  - can detect a maximum of 8 patterns in one image
    //tracker = new ARToolKitPlus::TrackerSingleMarkerImpl<16,16,16, 1, 8>(width,height);
    tracker = new TrackerSingleMarker (width, height, 8, 16, 16, 16, 1);
    //const char* description = tracker->getDescription();
    //printf("ARToolKitPlus compile-time information:\n%s\n\n", description);

    // set a logger so we can output error messages
    //tracker->setLogger(&logger);
    tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);

    // if ( !tracker->init( (const char *)ofToDataPath("LogitechPro4000.dat").c_str(), 1.0f, 5000.0f) )           // load std. ARToolKit camera file
    // {
    // printf("ERROR: init() failed\n");
    // delete cameraBuffer;
    // delete tracker;
    // return;
    // }// load a camera file.
    if (!tracker->init((const char *)ofToDataPath("Logitech_Notebook_Pro.cal").c_str(), 1.0f, 5000.0f)) // load MATLAB file
    {
        printf("ERROR: init() failed\n");
        printf("ERROR: init() failed\n");
        delete cameraBuffer;
        delete tracker;
        std::exit(-1);
    }
    //tracker->getCamera()->printSettings();
    // define size of the marker
    tracker->setPatternWidth(80);

    // the marker in the BCH test image has a thin border...
    tracker->setBorderWidth(useBCH ? 0.125f : 0.250f);

    // set a threshold. alternatively we could also activate automatic thresholding
    tracker->setThreshold(150);

    // let's use lookup-table undistortion for high-speed
    // note: LUT only works with images up to 1024x1024
    tracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);

    // RPP is more robust than ARToolKit's standard pose estimator
    //tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);

    // switch to simple ID based markers
    // use the tool in tools/IdPatGen to generate markers
    tracker->setMarkerMode(useBCH ? ARToolKitPlus::MARKER_ID_BCH : ARToolKitPlus::MARKER_ID_SIMPLE);
    tracker->setMarkerMode (ARToolKitPlus::MARKER_TEMPLATE);
    tracker->setPatternWidth (80);
    tracker->addPattern ((const char *)ofToDataPath("patt.hiro").c_str());

}

//--------------------------------------------------------------
void testApp::update()
{
    mWidth = ofGetWidth();
    mHeight = ofGetHeight();


    grabber.grabFrame();
    if (grabber.isFrameNew())
    {

        //convert our camera frame to grayscale
        convert.setFromPixels(grabber.getPixels(), 640, 480);
        gray = convert;

//        //find the marker and get back the confidence
        markerId = tracker->calc(gray.getPixels());
        tracker->selectBestMarkerByCf();
        float conf = tracker->getConfidence();
        if ( conf > 0.0 )
        {
            bDraw = true;
        }
        else bDraw = false;
        // use the result of calc() to setup the OpenGL transformation
        // glMatrixMode(GL_MODELVIEW)
        // glLoadMatrixf(tracker.getModelViewMatrix());

        if(markerId.size()>0)
        {
            printf("\n\nFound marker %d  (confidence %d%%)\n\nPose-Matrix:\n  ", markerId[0], (int(conf * 100.0f)));
            for (int i = 0; i < 16; i++)
            printf("%.2f  %s", tracker->getModelViewMatrix()[i], (i % 4 == 3) ? "\n  " : "");
        }
//        float conf = (float)tracker->getConfidence();
//
//        //if ( conf > 0.0 )
//        //{
//            bDraw = true;
//        //}
//        //else bDraw = false;
//#ifdef DEBUG
//       for(int i = 0 ; i < markerId.size() ; i++)
//       {
//           printf("\n\nFound marker %d  (confidence %d%%)\n\nPose-Matrix:\n  ", markerId[i], (int(conf*100.0f)));
//       }
//        //prints out the matrix - useful for debugging?
//        for (int i=0; i<16; i++)
//            printf("%.2f  %s", tracker->getModelViewMatrix()[i], (i%4==3)?"\n  " : "");
//#endif
    }
}

//--------------------------------------------------------------
void testApp::draw()
{
    ofSetColor(0xffffff);
    grabber.draw(0, 0,mWidth,mHeight);

    //this is where we use the calculated matrix from ARToolkitPlus to put
    //in our graphics at the location and orientation of the marker.
    //- the matrix has the 0,0 point as the center of the marker.

    if (bDraw)
    {

        glViewport(0, 0, mWidth, mHeight );
        glMatrixMode( GL_PROJECTION );
        glLoadMatrixf(tracker->getProjectionMatrix());
        glMatrixMode( GL_MODELVIEW );
        glLoadMatrixf(tracker->getModelViewMatrix());

        //ofSetColor(0xFFFFFF);
        glEnable (GL_DEPTH_TEST);


        glDisable(GL_DEPTH_TEST);
#ifdef DEBUG
        if (markerId.size()>0)
        {
            glPushMatrix();
            glRotatef(-90, 1, 0, 0);
            ofSetColor(220 , 20, 230 );

            AR.drawString("ARToolKitPlus "+ofToString(markerId[0]), -AR.stringWidth("ARToolKitPlus")/2,0);
            glPopMatrix();

            glPushMatrix();
            glRotatef(-90, 1, 0, 0);
            glRotatef(-90, 0, 1, 0);
            ofSetColor(20 , 180, 220 );
            OF.drawString("openFrameworks"+ofToString(markerId[0]), -OF.stringWidth("openFrameworks")/2,0);
            glPopMatrix();
            ofSetColor(255,255,255);
        }
#endif
    }
    //lets tumble the world with the mouse
    /*	glPushMatrix();

    //draw in middle of the screen
    glTranslatef(ofGetWidth()/2,ofGetHeight()/2,0);
    //tumble according to mouse
    glRotatef(-mouseY,1,0,0);
    glRotatef(mouseX,0,1,0);
    glTranslatef(-ofGetWidth()/2,-ofGetHeight()/2,0);

    ofSetColor(255, 255, 255, 255);


    glPopMatrix();*/

}


//--------------------------------------------------------------
void testApp::keyPressed  (int key)
{
     if (key == 's')grabber.videoSettings();
    switch (key)
    {

    }
}

//--------------------------------------------------------------
void testApp::keyReleased  (int key)
{
}

//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y )
{
}

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button)
{
}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button)
{
}

//--------------------------------------------------------------
void testApp::mouseReleased()
{

}
